#ifndef __MATH_QUAT_H__
#define __MATH_QUAT_H__

/*
===============================================================================

Quaternion

===============================================================================
*/

namespace inf {

    class Vec3;
    class Angles;
    class Rotation;
    class Mat3;
    class Mat4;
    class CQuat;

    class Quat {
    public:
        float			x;
        float			y;
        float			z;
        float			w;

        Quat( void );
        Quat( float x, float y, float z, float w );

        void 			set( float x, float y, float z, float w );

        float			operator[]( int index ) const;
        float &			operator[]( int index );
        Quat			operator-() const;
        Quat &		operator=( const Quat &a );
        Quat			operator+( const Quat &a ) const;
        Quat &		operator+=( const Quat &a );
        Quat			operator-( const Quat &a ) const;
        Quat &		operator-=( const Quat &a );
        Quat			operator*( const Quat &a ) const;
        Vec3			operator*( const Vec3 &a ) const;
        Quat			operator*( float a ) const;
        Quat &		operator*=( const Quat &a );
        Quat &		operator*=( float a );

        friend Quat	operator*( const float a, const Quat &b );
        friend Vec3	operator*( const Vec3 &a, const Quat &b );

        bool			compare( const Quat &a ) const;						// exact compare, no epsilon
        bool			compare( const Quat &a, const float epsilon ) const;	// compare with epsilon
        bool			operator==(	const Quat &a ) const;					// exact compare, no epsilon
        bool			operator!=(	const Quat &a ) const;					// exact compare, no epsilon

        Quat			inverse( void ) const;
        float			length( void ) const;
        Quat &			normalize( void );

        float			calcW( void ) const;
        int				get_dimension( void ) const;

        Angles			to_Angles( void ) const;
        Rotation		to_Rotation( void ) const;
        Mat3			to_Mat3( void ) const;
        Mat4			to_Mat4( void ) const;
        CQuat			to_CQuat( void ) const;
        Vec3			to_angular_velocity( void ) const;
        const float *	to_float_ptr( void ) const;
        float *			to_float_ptr( void );
        const char *	to_string( int precision = 2 ) const;

        Quat &		    slerp( const Quat &from, const Quat &to, float t );
        Quat &		    slerp_fast( const Quat &from, const Quat &to, float t );

        bool			fix_denormals( float epsilon = math::FLOAT_EPSILON );				// change tiny numbers to zero

        static const Quat ORIGIN;
        static const Quat ONE;
    };

    INF_INLINE Quat::Quat( void ) {
    }

    INF_INLINE Quat::Quat( float x, float y, float z, float w ) {
        this->x = x;
        this->y = y;
        this->z = z;
        this->w = w;
    }

    INF_INLINE float Quat::operator[]( int index ) const {
        assert( ( index >= 0 ) && ( index < 4 ) );
        return ( &x )[ index ];
    }

    INF_INLINE float& Quat::operator[]( int index ) {
        assert( ( index >= 0 ) && ( index < 4 ) );
        return ( &x )[ index ];
    }

    INF_INLINE Quat Quat::operator-( void ) const {
        return Quat( -x, -y, -z, -w );
    }

    INF_INLINE Quat &Quat::operator=( const Quat &a ) {
        x = a.x;
        y = a.y;
        z = a.z;
        w = a.w;

        return *this;
    }

    INF_INLINE Quat Quat::operator+( const Quat &a ) const {
        return Quat( x + a.x, y + a.y, z + a.z, w + a.w );
    }

    INF_INLINE Quat& Quat::operator+=( const Quat &a ) {
        x += a.x;
        y += a.y;
        z += a.z;
        w += a.w;

        return *this;
    }

    INF_INLINE Quat Quat::operator-( const Quat &a ) const {
        return Quat( x - a.x, y - a.y, z - a.z, w - a.w );
    }

    INF_INLINE Quat& Quat::operator-=( const Quat &a ) {
        x -= a.x;
        y -= a.y;
        z -= a.z;
        w -= a.w;

        return *this;
    }

    INF_INLINE Quat Quat::operator*( const Quat &a ) const {
        return Quat(	w*a.x + x*a.w + y*a.z - z*a.y,
            w*a.y + y*a.w + z*a.x - x*a.z,
            w*a.z + z*a.w + x*a.y - y*a.x,
            w*a.w - x*a.x - y*a.y - z*a.z );
    }

    INF_INLINE Vec3 Quat::operator*( const Vec3 &a ) const {
#if 0
        // it's faster to do the conversion to a 3x3 matrix and multiply the vector by this 3x3 matrix
        return ( to_Mat3() * a );
#else
        // result = this->inverse() * Quat( a.x, a.y, a.z, 0.0f ) * (*this)
        float xxzz = x*x - z*z;
        float wwyy = w*w - y*y;

        float xw2 = x*w*2.0f;
        float xy2 = x*y*2.0f;
        float xz2 = x*z*2.0f;
        float yw2 = y*w*2.0f;
        float yz2 = y*z*2.0f;
        float zw2 = z*w*2.0f;

        return Vec3(
            (xxzz + wwyy)*a.x		+ (xy2 + zw2)*a.y		+ (xz2 - yw2)*a.z,
            (xy2 - zw2)*a.x			+ (xxzz - wwyy)*a.y		+ (yz2 + xw2)*a.z,
            (xz2 + yw2)*a.x			+ (yz2 - xw2)*a.y		+ (wwyy - xxzz)*a.z
            );
#endif
    }

    INF_INLINE Quat Quat::operator*( float a ) const {
        return Quat( x * a, y * a, z * a, w * a );
    }

    INF_INLINE Quat operator*( const float a, const Quat &b ) {
        return b * a;
    }

    INF_INLINE Vec3 operator*( const Vec3 &a, const Quat &b ) {
        return b * a;
    }

    INF_INLINE Quat& Quat::operator*=( const Quat &a ) {
        *this = *this * a;

        return *this;
    }

    INF_INLINE Quat& Quat::operator*=( float a ) {
        x *= a;
        y *= a;
        z *= a;
        w *= a;

        return *this;
    }

    INF_INLINE bool Quat::compare( const Quat &a ) const {
        return ( ( x == a.x ) && ( y == a.y ) && ( z == a.z ) && ( w == a.w ) );
    }

    INF_INLINE bool Quat::compare( const Quat &a, const float epsilon ) const {
        if ( math::fabs( x - a.x ) > epsilon ) {
            return false;
        }
        if ( math::fabs( y - a.y ) > epsilon ) {
            return false;
        }
        if ( math::fabs( z - a.z ) > epsilon ) {
            return false;
        }
        if ( math::fabs( w - a.w ) > epsilon ) {
            return false;
        }
        return true;
    }

    INF_INLINE bool Quat::operator==( const Quat &a ) const {
        return compare( a );
    }

    INF_INLINE bool Quat::operator!=( const Quat &a ) const {
        return !compare( a );
    }

    INF_INLINE void Quat::set( float x, float y, float z, float w ) {
        this->x = x;
        this->y = y;
        this->z = z;
        this->w = w;
    }

    INF_INLINE Quat Quat::inverse( void ) const {
        return Quat( -x, -y, -z, w );
    }

    INF_INLINE float Quat::length( void ) const {
        float len;

        len = x * x + y * y + z * z + w * w;
        return math::sqrt( len );
    }

    INF_INLINE Quat& Quat::normalize( void ) {
        float len;
        float ilength;

        len = this->length();
        if ( len ) {
            ilength = 1.0f / len;
            x *= ilength;
            y *= ilength;
            z *= ilength;
            w *= ilength;
        }
        return *this;
    }

    INF_INLINE float Quat::calcW( void ) const {
        // take the absolute value because floating point rounding may cause the dot of x,y,z to be larger than 1
        return math::sqrt( math::fabs( 1.0f - ( x * x + y * y + z * z ) ) );
    }

    INF_INLINE int Quat::get_dimension( void ) const {
        return 4;
    }

    INF_INLINE const float *Quat::to_float_ptr( void ) const {
        return &x;
    }

    INF_INLINE float *Quat::to_float_ptr( void ) {
        return &x;
    }

    INF_INLINE bool Quat::fix_denormals( float epsilon ) {
        bool denormal = false;
        if ( math::fabs( x ) < epsilon ) {
            x = 0.0f;
            denormal = true;
        }
        if ( math::fabs( y ) < epsilon ) {
            y = 0.0f;
            denormal = true;
        }
        if ( math::fabs( z ) < epsilon ) {
            z = 0.0f;
            denormal = true;
        }
        if ( math::fabs( w ) < epsilon ) {
            w = 0.0f;
            denormal = true;
        }
        return denormal;
    }

    /*
    ===============================================================================

    Compressed quaternion

    ===============================================================================
    */

    class CQuat {
    public:
        float			x;
        float			y;
        float			z;

        CQuat( void );
        CQuat( float x, float y, float z );

        void 			set( float x, float y, float z );

        float			operator[]( int index ) const;
        float &			operator[]( int index );

        bool			compare( const CQuat &a ) const;						// exact compare, no epsilon
        bool			compare( const CQuat &a, const float epsilon ) const;	// compare with epsilon
        bool			operator==(	const CQuat &a ) const;					// exact compare, no epsilon
        bool			operator!=(	const CQuat &a ) const;					// exact compare, no epsilon

        int				get_dimension( void ) const;

        Angles		    to_Angles( void ) const;
        Rotation		to_Rotation( void ) const;
        Mat3			to_Mat3( void ) const;
        Mat4			to_Mat4( void ) const;
        Quat			to_Quat( void ) const;
        const float *	to_float_ptr( void ) const;
        float *			to_float_ptr( void );
        const char *	to_string( int precision = 2 ) const;

        bool			fix_denormals( float epsilon = math::FLOAT_EPSILON );	// change tiny numbers to zero
    };

    INF_INLINE CQuat::CQuat( void ) {
    }

    INF_INLINE CQuat::CQuat( float x, float y, float z ) {
        this->x = x;
        this->y = y;
        this->z = z;
    }

    INF_INLINE void CQuat::set( float x, float y, float z ) {
        this->x = x;
        this->y = y;
        this->z = z;
    }

    INF_INLINE float CQuat::operator[]( int index ) const {
        assert( ( index >= 0 ) && ( index < 3 ) );
        return ( &x )[ index ];
    }

    INF_INLINE float& CQuat::operator[]( int index ) {
        assert( ( index >= 0 ) && ( index < 3 ) );
        return ( &x )[ index ];
    }

    INF_INLINE bool CQuat::compare( const CQuat &a ) const {
        return ( ( x == a.x ) && ( y == a.y ) && ( z == a.z ) );
    }

    INF_INLINE bool CQuat::compare( const CQuat &a, const float epsilon ) const {
        if ( math::fabs( x - a.x ) > epsilon ) {
            return false;
        }
        if ( math::fabs( y - a.y ) > epsilon ) {
            return false;
        }
        if ( math::fabs( z - a.z ) > epsilon ) {
            return false;
        }
        return true;
    }

    INF_INLINE bool CQuat::operator==( const CQuat &a ) const {
        return compare( a );
    }

    INF_INLINE bool CQuat::operator!=( const CQuat &a ) const {
        return !compare( a );
    }

    INF_INLINE int CQuat::get_dimension( void ) const {
        return 3;
    }

    INF_INLINE Quat CQuat::to_Quat( void ) const {
        // take the absolute value because floating point rounding may cause the dot of x,y,z to be larger than 1
        return Quat( x, y, z, math::sqrt( math::fabs( 1.0f - ( x * x + y * y + z * z ) ) ) );
    }

    INF_INLINE const float *CQuat::to_float_ptr( void ) const {
        return &x;
    }

    INF_INLINE float *CQuat::to_float_ptr( void ) {
        return &x;
    }

    INF_INLINE bool CQuat::fix_denormals( float epsilon ) {
        bool denormal = false;
        if ( math::fabs( x ) < epsilon ) {
            x = 0.0f;
            denormal = true;
        }
        if ( math::fabs( y ) < epsilon ) {
            y = 0.0f;
            denormal = true;
        }
        if ( math::fabs( z ) < epsilon ) {
            z = 0.0f;
            denormal = true;
        }
        return denormal;
    }


}   // ::inf

#endif /* !__MATH_QUAT_H__ */
